// TODO cenasverdesavoar

static unsigned int matrix_coefficients = 6;

static int Inverse_Table_6_9[8][4] =
{
  {117504, 138453, 13954, 34903}, /* no sequence_display_extension */
  {117504, 138453, 13954, 34903}, /* ITU-R Rec. 709 (1990) */
  {104597, 132201, 25675, 53279}, /* unspecified */
  {104597, 132201, 25675, 53279}, /* reserved */
  {104448, 132798, 24759, 53109}, /* FCC */
  {104597, 132201, 25675, 53279}, /* ITU-R Rec. 624-4 System B, G */
  {104597, 132201, 25675, 53279}, /* SMPTE 170M */
  {117579, 136230, 16907, 35559}  /* SMPTE 240M (1987) */
};

static unsigned char clip (int y, int common)
{
    int i = (y + common) >> 16;
    return (unsigned char) (i < 0 ? 0 : (i > 255 ? 255 : i));
}

static void YUV2RGB420_24(unsigned char* image,
        const unsigned char* py,
        const unsigned char* pu,
        const unsigned char* pv,
        const unsigned int width,
        const unsigned int height,
        const unsigned int y_stride,
        const unsigned int uv_stride)
{
  int Y,U,V;
  int r_common,g_common,b_common;
  unsigned int x,y;
  unsigned int rgb_stride = 3*width;
  
  unsigned char *dst_line_1;
  unsigned char *dst_line_2;
  const unsigned char* py_line_1;
  const unsigned char* py_line_2;
  
  int crv,cbu,cgu,cgv;
  
  /* matrix coefficients */
  crv = Inverse_Table_6_9[matrix_coefficients][0];
  cbu = Inverse_Table_6_9[matrix_coefficients][1];
  cgu = Inverse_Table_6_9[matrix_coefficients][2];
  cgv = Inverse_Table_6_9[matrix_coefficients][3];
	
  dst_line_1 = image;
  dst_line_2 = dst_line_1 + rgb_stride;
  
  py_line_1 = py;
  py_line_2 = py + y_stride;
  
  for (y = 0; y < height/2; y++) 
  {
    for (x = 0; x < width/2; x++) 
	{

	  //Common to all four pixels
	  U = (*pu++) - 128;
	  V = (*pv++) - 128;

	  r_common = crv * V + 32768;
	  g_common = cgu * U + cgu * V - 32768;
	  b_common = cbu * U + 32768;

	  //Pixel I
	  Y = 76309 * ((*py_line_1++) - 16);
	  *dst_line_1++ = clip(Y, r_common);
	  *dst_line_1++ = clip(Y, -g_common);
	  *dst_line_1++ = clip(Y, b_common);
		  
	  //Pixel II
	  Y = 76309 * ((*py_line_1++) - 16);
	  *dst_line_1++ = clip(Y, r_common);
	  *dst_line_1++ = clip(Y, -g_common);
	  *dst_line_1++ = clip(Y, b_common);

	  //Pixel III
	  Y = 76309 * ((*py_line_2++) - 16);
	  *dst_line_2++ = clip(Y, r_common);
	  *dst_line_2++ = clip(Y, -g_common);
	  *dst_line_2++ = clip(Y, b_common);

	  //Pixel IV
	  Y = 76309 * ((*py_line_2++) - 16);
	  *dst_line_2++ = clip(Y, r_common);
	  *dst_line_2++ = clip(Y, -g_common);
	  *dst_line_2++ = clip(Y, b_common);
	}

      py_line_1 += y_stride;
      py_line_2 += y_stride;
      pu += uv_stride - width/2;
      pv += uv_stride - width/2;
      dst_line_1 += rgb_stride;
      dst_line_2 += rgb_stride;
  }
}
